
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       is_flying.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "logger.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <notify/notify.h>
#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/
#define CRASH_DETECTION_DELAY_MS            500
#define IS_FLYING_IMPACT_TIMER_MS           3000
#define GPS_IS_FLYING_SPEED_CMS             150
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  Do we think we are flying?
  Probabilistic method where a bool is low-passed and considered a probability.
*/
void fms_update_is_flying_5Hz(void)
{
    uitc_sensor_gps vehicle_gps_position;
    itc_copy_from_hub(ITC_ID(sensor_gps),  &vehicle_gps_position);

    float aspeed=0;
    bool is_flying_bool = false;
    uint32_t now_ms = time_millis();

    uint32_t ground_speed_thresh_cm = (fms.aparm.min_gndspeed_cm > 0) ? ((uint32_t)(fms.aparm.min_gndspeed_cm*0.9f)) : GPS_IS_FLYING_SPEED_CMS;
    bool gps_confirmed_movement = (vehicle_gps_position.fix_type >= uITC_GPS_OK_FIX_3D) &&
                                    (vehicle_gps_position.vel_m_s * 100 >= ground_speed_thresh_cm);

    // airspeed at least 75% of stall speed?
    const float airspeed_threshold = MAX(fms.aparm.airspeed_min,2)*0.75f;
    bool airspeed_movement = ahrs_airspeed_estimate(fms.ahrs, &aspeed) && (aspeed >= airspeed_threshold);

    if (vehicle_gps_position.fix_type < uITC_GPS_OK_FIX_2D && mb_arming_is_armed() && !airspeed_movement && fms.isFlyingProbability > 0.3f) {
        // when flying with no GPS, use the last airspeed estimate to
        // determine if we think we have airspeed movement. This
        // prevents the crash detector from triggering when
        // dead-reckoning under long GPS loss
        airspeed_movement = aspeed >= airspeed_threshold;
    }

#if HAL_QUADPLANE_ENABLED
    is_flying_bool = quadplane.is_flying();
#endif
    if (is_flying_bool) {
        // no need to look further
    } else if(mb_arming_is_armed()) {
        // when armed assuming flying and we need overwhelming evidence that we ARE NOT flying
        // short drop-outs of GPS are common during flight due to banking which points the antenna in different directions
        bool gps_lost_recently = (vehicle_gps_position.last_fix_time_us*0.001f > 0) && // we have locked to GPS before
                        (vehicle_gps_position.fix_type < uITC_GPS_OK_FIX_2D) && // and it's lost now
                        (now_ms - vehicle_gps_position.last_fix_time_us*0.001f < 5000); // but it wasn't that long ago (<5s)

        if ((fms.auto_state.last_flying_ms > 0) && gps_lost_recently) {
            // we've flown before, remove GPS constraints temporarily and only use airspeed
            is_flying_bool = airspeed_movement; // moving through the air
        } else {
            // Because ahrs.airspeed_estimate can return a continued high value after landing if flying in
            // strong winds above stall speed it is necessary to include the IMU based movement check.
            is_flying_bool = (airspeed_movement && !ahrs_ins_is_still(fms.ahrs)) || // moving through the air
                                gps_confirmed_movement; // locked and we're moving
        }

        if (fms.control_mode == (mode_base_t)&fms.mode_auto) {
            /*
              make is_flying() more accurate during various auto modes
             */

            // Detect X-axis deceleration for probable ground impacts.
            // Limit the max probability so it can decay faster. This
            // will not change the is_flying state, anything above 0.1
            // is "true", it just allows it to decay faster once we decide we
            // aren't flying using the normal schemes
            if (fms.g.crash_accel_threshold == 0) {
                fms.crash_state.impact_detected = false;
            } else if (fms.ahrs->accel_peak_hold_neg_x < -(fms.g.crash_accel_threshold)) {
                // large deceleration detected, lets lower confidence VERY quickly
                fms.crash_state.impact_detected = true;
                fms.crash_state.impact_timer_ms = now_ms;
                if (fms.isFlyingProbability > 0.2f) {
                    fms.isFlyingProbability = 0.2f;
                }
            } else if (fms.crash_state.impact_detected &&
                (now_ms - fms.crash_state.impact_timer_ms > IS_FLYING_IMPACT_TIMER_MS)) {
                // no impacts seen in a while, clear the flag so we stop clipping isFlyingProbability
                fms.crash_state.impact_detected = false;
            }

            switch (fms.flight_stage)
            {
            case FWFS_TAKEOFF:
                break;

            case FWFS_NORMAL:
                if (fms_in_preLaunch_flight_stage()) {
                    // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so
                    // ensure we aren't showing a false positive.
                    is_flying_bool = false;
                    fms.crash_state.is_crashed = false;
                    fms.auto_state.started_flying_in_auto_ms = 0;
                }
                break;

            case FWFS_VTOL:
                // TODO: detect ground impacts
                break;

            case FWFS_LAND:
                if (landing_is_on_approach() && fms.auto_state.sink_rate > 0.2f) {
                    is_flying_bool = true;
                }
                break;

            case FWFS_ABORT_LANDING:
                if (fms.auto_state.sink_rate < -0.5f) {
                    // steep climb
                    is_flying_bool = true;
                }
                break;

            default:
                break;
            } // switch
        }
    } else {
        // when disarmed assume not flying and need overwhelming evidence that we ARE flying
        is_flying_bool = airspeed_movement && gps_confirmed_movement;
#if 0
        if ((fms.flight_stage == FWFS_TAKEOFF) || landing.is_flaring()) {
            is_flying_bool = false;
        }
#else
        if (fms.flight_stage == FWFS_TAKEOFF) {
            is_flying_bool = false;
        }
#endif
    }

    if (!fms.crash_state.impact_detected || !is_flying_bool) {
        // when impact is detected, enforce a clip. Only allow isFlyingProbability to go down, not up.
        // low-pass the result.
        // coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%)
        fms.isFlyingProbability = (0.85f * fms.isFlyingProbability) + (0.15f * (float)is_flying_bool);
    }

    /*
      update last_flying_ms so we always know how long we have not
      been flying for. This helps for crash detection and auto-disarm
     */
    bool new_is_flying = fms_is_flying();

    // we are flying, note the time
    if (new_is_flying) {

        fms.auto_state.last_flying_ms = now_ms;

        if (!fms.previous_is_flying) {
            // just started flying in any mode
            fms.started_flying_ms = now_ms;
        }

        if ((fms.control_mode == (mode_base_t)&fms.mode_auto) &&
            ((fms.auto_state.started_flying_in_auto_ms == 0) || !fms.previous_is_flying) ) {

            // We just started flying, note that time also
            fms.auto_state.started_flying_in_auto_ms = now_ms;
        }
    }
    fms.previous_is_flying = new_is_flying;

    notify_flags.flying = new_is_flying;

    fms_crash_detection_update(&vehicle_gps_position);

    //aplog_write_plane_status();

    // tell AHRS flying state
    //fms_set_likely_flying(new_is_flying);

    // conservative ground mode value for rate D suppression
    fms.ground_mode = !fms_is_flying() && !mb_arming_is_armed_and_safety_off();
}

/*
 * Determine if we have crashed
 */
void fms_crash_detection_update(void *uitc_gps)
{
    uitc_sensor_gps *vehicle_gps_position = (uitc_sensor_gps *)uitc_gps;

    if (fms.control_mode != (mode_base_t)&fms.mode_auto || !fms.aparm.crash_detection_enable)
    {
        // crash detection is only available in AUTO mode
        fms.crash_state.debounce_timer_ms = 0;
        fms.crash_state.is_crashed = false;
        return;
    }

    uint32_t now_ms = time_millis();
    bool crashed_near_land_waypoint = false;
    bool crashed = false;
    bool been_auto_flying = (fms.auto_state.started_flying_in_auto_ms > 0) &&
                            (now_ms - fms.auto_state.started_flying_in_auto_ms >= 2500);

    if (!fms_is_flying() && mb_arming_is_armed())
    {
        if (landing_is_expecting_impact()) {
            // We should be nice and level-ish in this flight stage. If not, we most
            // likely had a crazy landing. Throttle is inhibited already at the flare
            // but go ahead and notify GCS and perform any additional post-crash actions.
            // Declare a crash if we are oriented more that 60deg in pitch or roll
            if (!fms.crash_state.checkedHardLanding && // only check once
                been_auto_flying &&
                (labs(fms.ahrs->roll_sensor_cd) > 6000 || labs(fms.ahrs->pitch_sensor_cd) > 6000)) {
                
                crashed = true;

                // did we "crash" within 75m of the landing location? Probably just a hard landing
                crashed_near_land_waypoint =
                        location_get_distance(&fms.current_loc , &(mission_get_current_nav_cmd()->content.location)) < 75;

                // trigger hard landing event right away, or never again. This inhibits a false hard landing
                // event when, for example, a minute after a good landing you pick the plane up and
                // this logic is still running and detects the plane is on its side as you carry it.
                fms.crash_state.debounce_timer_ms = now_ms;
                fms.crash_state.debounce_time_total_ms = 0; // no debounce
            }

            fms.crash_state.checkedHardLanding = true;

        } else if (landing_is_on_approach()) {
            // when altitude gets low, we automatically flare so ground crashes
            // most likely can not be triggered from here. However,
            // a crash into a tree would be caught here.
            if (been_auto_flying) {
                crashed = true;
                fms.crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
            }

        } else {
            switch (fms.flight_stage)
            {
            case FWFS_TAKEOFF:
                if (fms.g.takeoff_throttle_min_accel > 0 &&
                        !fms.throttle_suppressed) {
                    // if you have an acceleration holding back throttle, but you met the
                    // accel threshold but still not fying, then you either shook/hit the
                    // plane or it was a failed launch.
                    crashed = true;
                    fms.crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
                }
                // TODO: handle auto missions without NAV_TAKEOFF mission cmd
                break;

            case FWFS_NORMAL:
                if (!fms_in_preLaunch_flight_stage() && been_auto_flying) {
                    crashed = true;
                    fms.crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
                }
                break;

            case FWFS_VTOL:
                // we need a totally new method for this
                crashed = false;
                break;

            default:
                break;
            } // switch
        }
    } else {
        fms.crash_state.checkedHardLanding = false;
    }

    // if we have no GPS lock and we don't have a functional airspeed
    // sensor then don't do crash detection
    if (vehicle_gps_position->fix_type < uITC_GPS_OK_FIX_3D) {
#if AP_AIRSPEED_ENABLED
        if (!airspeed.use() || !airspeed.healthy()) {
            crashed = false;
        }
#else
        crashed = false;
#endif
    }

    if (!crashed) {
        // reset timer
        fms.crash_state.debounce_timer_ms = 0;

    } else if (fms.crash_state.debounce_timer_ms == 0) {
        // start timer
        fms.crash_state.debounce_timer_ms = now_ms;

    } else if ((now_ms - fms.crash_state.debounce_timer_ms >= fms.crash_state.debounce_time_total_ms) && !fms.crash_state.is_crashed) {
        fms.crash_state.is_crashed = true;
        if (fms.aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
            mb_arming_disarm(ARMING_METHOD_CRASH, true);
        }
        if (crashed_near_land_waypoint) {
            gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
        } else {
            gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected");
        }
    }
}

/*
  return true if we think we are flying. This is a probabilistic
  estimate, and needs to be used very carefully. Each use case needs
  to be thought about individually.
 */
bool fms_is_flying(void)
{
    if (mb_arming_is_armed_and_safety_off()) {
#if HAL_QUADPLANE_ENABLED
        if (quadplane.is_flying_vtol()) {
            return true;
        }
#endif
        // when armed, assume we're flying unless we probably aren't
        return (fms.isFlyingProbability >= 0.1f);
    }

    // when disarmed, assume we're not flying unless we probably are
    return (fms.isFlyingProbability >= 0.9f);
}

/*
 * return true if we are in a pre-launch phase of an auto-launch, typically used in bungee launches
 */
bool fms_in_preLaunch_flight_stage(void)
{
    if (fms.control_mode == (mode_base_t)&fms.mode_takeoff && fms.throttle_suppressed) {
        return true;
    }
#if HAL_QUADPLANE_ENABLED
    if (quadplane.is_vtol_takeoff(mission_get_current_nav_cmd()->id)) {
        return false;
    }
#endif
    return (fms.control_mode == (mode_base_t)&fms.mode_auto &&
            fms.throttle_suppressed &&
            fms.flight_stage == FWFS_NORMAL &&
            mission_get_current_nav_cmd()->id == MAV_CMD_NAV_TAKEOFF);
}

/*------------------------------------test------------------------------------*/


